import time
from multi_robomaster import multi_robot
import threading

from ep_staticatch_rule import EP
import pos_nokov as pos
import center_control as cc
import udp_client as uc

def run(p, v):
    ut=threading.Thread(target=uc.main, args=(p, v))
    ut.start()
    robots_sn_list = ['3JKDH2T00159G8','3JKCJC400302GS','3JKCJC400301ZP', 
                      '3JKCJC400301UD','3JKCJC400301W0', '3JKCJC400300Y9']

    multi_robots = multi_robot.MultiEP()
    multi_robots.initialize()
    
    number = multi_robots.number_id_by_sn([2, robots_sn_list[1]],
                                          [3, robots_sn_list[2]])
    print("The number of robot is: {0}".format(number))
    g2 = multi_robots.build_group([2])
    g3 = multi_robots.build_group([3])

    
    ep2=EP(multi_robots, g2, id=2, role='chase')
    ep3=EP(multi_robots, g3, id=3)

    t2 = threading.Thread(target=ep2.run,args=(p,v))
    t2.start()
    t3 = threading.Thread(target=ep3.run,args=(p,v))
    t3.start()

